#include "auborobot.h"
#include <QSequentialIterable>
#include <QCoreApplication>
#include <QTime>

#define PI 3.1415926

AuboRobot::AuboRobot(QObject *parent)
    : Device{parent}
    , m_strIP("192.168.1.1")
    , m_nPort(1000)
    , m_dblVelocity(0.5)
    , m_dblAcceleration(3)
{
    m_dblJointRadian[0] = 0;
    m_dblJointRadian[1] = 0;
    m_dblJointRadian[2] = 0;
    m_dblJointRadian[3] = 0;
    m_dblJointRadian[4] = 0;
    m_dblJointRadian[5] = 0;
}

int AuboRobot::initDevice(){
    int ret = rs_initialize();
    if(RS_SUCC == ret){
        qDebug()<<"设备初始化成功!";

        //rs_robot_startup()
        rs_create_context(&m_rshd);

        //ret = rs_login(m_rshd,"192.168.159.128",8899);
        ret = rs_login(m_rshd,m_strIP.toLatin1(),m_nPort);

        //rs_move_joint(m_rshd,m_dblJointRadian,true);
    }

    return 0;
}

bool AuboRobot::isInPos(double* dest_pos,double* cur_pos,double theshold){
    double sum = 0;
    for(int i = 0;i < ARM_DOF;i ++){
        sum += qAbs(dest_pos[i] - cur_pos[i]);
    }
    return sum < theshold ? true : false;
}

void AuboRobot::SetVelocity(double value){
    m_dblVelocity = value;

    JointVelcAccParam* pJointVelcAccParam = new JointVelcAccParam();
    for(int i = 0 ; i < 6;i++){
        pJointVelcAccParam->jointPara[i] = value;
    }

    rs_set_global_joint_maxvelc(m_rshd,pJointVelcAccParam);// 设置六关节最大角速度
}

void AuboRobot::SetAcceleration(double value){
    m_dblAcceleration = value;

    JointVelcAccParam* pJointVelcAccParam = new JointVelcAccParam();
    for(int i = 0 ; i < 6;i++){
        pJointVelcAccParam->jointPara[i] = value;
    }

    rs_set_global_joint_maxacc(m_rshd,pJointVelcAccParam);// 设置六关节最大角速度

}

void AuboRobot::writeData(const QVariant &data)
{
    RobotState state = RobotRunning;
    //JointValue cur_jval {0};
    if (data.canConvert<QVariantList>()) {
        QSequentialIterable iterable = data.value<QSequentialIterable>();
        //foreach方式遍历:
        int index = 0;
        // 将六个关节角度转换为弧度，并形成一个数组
        foreach (const QVariant &v, iterable) {
            // 六个为关节角度
            if(index < 6){
                m_dblJointRadian[index] = PI * v.toDouble() / 180.0;
            }
            // 角速度
            if(index == 6){
                SetVelocity(v.toDouble());
            }
            // 角加速度
            if(index == 7){
                SetAcceleration(v.toDouble());
            }

            index++;
        }

        // 非阻塞
        //m_robot.joint_move(&m_joint_pos,ABS,FALSE,1);
        rs_move_joint(m_rshd,m_dblJointRadian,false);
        // 线程延时10毫秒，以便等待机械臂先运动
        QTime dieTime = QTime::currentTime().addMSecs(10);
        while (QTime::currentTime() < dieTime)
            QCoreApplication::processEvents(QEventLoop::AllEvents, 10);

        while(true){
            rs_get_robot_state(m_rshd,&state);
            QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
            if(state == RobotStopped){
                break;
            }
        }
        // 阻塞
        /*rs_move_joint(m_rshd,m_dblJointRadian,true);*/
    }

}

void AuboRobot::uninitDevice(){

    return;
}

void AuboRobot::writeXML(QXmlStreamWriter &writer)
{
    // 写入当前操作的信息
    writer.writeStartElement("AuboRobot");
    writer.writeAttribute(QStringLiteral("id"), m_strID);

    writer.writeTextElement("ip", m_strIP);
    writer.writeTextElement("port", QString::number(m_nPort));
    writer.writeEndElement(); // 结束当前操作的元素
}

void AuboRobot::readFromXML(QXmlStreamReader &reader)
{
    QXmlStreamAttributes attributes = reader.attributes();
    if (attributes.hasAttribute("id")) {
        m_strID = attributes.value("id").toString();
    }

    QXmlStreamReader::TokenType token = reader.readNext();
    while (!(reader.tokenType() == QXmlStreamReader::EndElement
             && reader.name() == QStringLiteral("AuboRobot"))) {
        if (reader.name() == QStringLiteral("ip")) {
            m_strIP = reader.readElementText();
        } else if (reader.name() == QStringLiteral("port")) {
            m_nPort = reader.readElementText().toInt();
        }

        reader.readNext();
    }
}

QString AuboRobot::getClassName()
{
    return this->metaObject()->className();
}

QString AuboRobot::strIP() const
{
    return m_strIP;
}

void AuboRobot::setStrIP(const QString &newStrIP)
{
    if (m_strIP == newStrIP)
        return;
    m_strIP = newStrIP;
    emit strIPChanged();
}

int AuboRobot::nPort() const
{
    return m_nPort;
}

void AuboRobot::setNPort(int newNPort)
{
    if (m_nPort == newNPort)
        return;
    m_nPort = newNPort;
    emit nPortChanged();
}
